%object
%aaa={nine_a_foura(9,:) nine_a_fourb(9,:) nine_a_fourc(9,:)};
%aaa={eight_b_foura(9,:) eight_b_fourb(9,:) eight_b_fourc(9,:)};
%aaa={four_a_threea(9,:) four_a_threeb(9,:) four_a_threec(9,:)};
%aaa={nine_d_foura(9,:) nine_d_fourb(9,:) nine_d_fourc(9,:)};
%aaa={nine_d_threea(9,:) nine_d_threeb(9,:) nine_d_threec(9,:)};

%no object
%aaa={nine_c_foura(9,:) nine_c_fourb(9,:) nine_c_fourc(9,:)};
%aaa={nine_a_threea(9,:) nine_a_threeb(9,:) nine_a_threec(9,:)};
%aaa={nine_a_twoa(9,:) nine_a_twob(9,:) nine_a_twoc(9,:) nine_a_twod(9,:)};
%aaa={nine_a_twob(9,:)  nine_a_twoc(9,:)  nine_a_twod(9,:)};
%aaa={nine_d_twoa(9,:) nine_d_twob(9,:) nine_d_twoc(9,:)};

aaa={aaa1(9,:) aaa2(9,:) aaa3(9,:)};
max_aaa=[max(aaa{1}),max(aaa{2}),max(aaa{3})];
min_aaa=[min(aaa{1}),min(aaa{2}),min(aaa{3})];
maxlim=max(max_aaa);
minlim=min(min_aaa);


for knd=1:3
    
    
    last_turn=1;
    turn_u=0;
    turn_d=0;
    refract=0;
    dir_ind=zeros(length(aaa{knd}),1);
    
    for ind=1:length(aaa{knd})
        
        if refract==1
            if abs(aaa{knd}(ind))<1
                refract=0;
            end
        end
        
        if refract==0
            if aaa{knd}(ind)<minlim+0.1
                turn_u=1;
                refract=1;
            elseif aaa{knd}(ind)>maxlim-0.1
                turn_d=1;
                refract=1;
            end
        end
        
        if turn_u
            dir_ind(last_turn:ind)=1;
            turn_u=0;
            last_turn=ind;
        elseif turn_d
            turn_d=0;
            last_turn=ind;
        end
    end
    
    
    dir_ind_all{knd}=logical(dir_ind);
    
end

aa=[];
dir_ind=[];
for knd=1:3
    aa(end+1:end+length(aaa{knd}))=aaa{knd};
    dir_ind(end+1:end+length(dir_ind_all{knd}))=dir_ind_all{knd};
end

dir_ind=logical(dir_ind);


figure;
plot(aa)
hold on
% plot(dir_ind,'r')
xlabel('Time [s]')
ylabel('Track position')
set(gca,'xtick',[0:1e5:1e6],'xticklabel',[0:100:1000])
set(gca,'ytick',[-5:2:5],'yticklabel',[0:20:100])
ylim([-6 6])

% aa=track_pos;

nbins=100;

bins=[minlim:(maxlim-minlim)/nbins:maxlim];

occ_count=[];

for ind=1:nbins
    occ_count_u(ind)=sum(aa(dir_ind)>=bins(ind)&aa(dir_ind)<bins(ind+1));
    occ_count_d(ind)=sum(aa(~dir_ind)>=bins(ind)&aa(~dir_ind)<bins(ind+1));
end

figure
subplot(3,1,1)
hold on
plot(occ_count_u/length(aa),'LineWidth',2)
plot(occ_count_d/length(aa),'r','LineWidth',2)
xlabel('Track position')
ylabel('Fraction of time')


aa_f=aa(1:round(length(aa)/4));
dir_ind_f=dir_ind(1:round(length(aa)/4));

occ_count=[];

for ind=1:nbins
    occ_count_u(ind)=sum(aa_f(dir_ind_f)>=bins(ind)&aa_f(dir_ind_f)<bins(ind+1));
    occ_count_d(ind)=sum(aa_f(~dir_ind_f)>=bins(ind)&aa_f(~dir_ind_f)<bins(ind+1));
end

subplot(3,1,2)
hold on
plot(occ_count_u/length(aa_f),'LineWidth',2)
plot(occ_count_d/length(aa_f),'r','LineWidth',2)
xlabel('Track position')
ylabel('Fraction of time')

aa_l=aa(end-round(length(aa)/4):end);
dir_ind_l=dir_ind(end-round(length(aa)/4):end);

occ_count=[];

for ind=1:nbins
    occ_count_u(ind)=sum(aa_l(dir_ind_l)>=bins(ind)&aa_l(dir_ind_l)<bins(ind+1));
    occ_count_d(ind)=sum(aa_l(~dir_ind_l)>=bins(ind)&aa_l(~dir_ind_l)<bins(ind+1));
end

subplot(3,1,3)
hold on
p1=plot(occ_count_u/length(aa_l),'LineWidth',2);
p2=plot(occ_count_d/length(aa_l),'r','LineWidth',2);
xlabel('Track position')
ylabel('Fraction of time')
